﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Windows.Forms;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;

namespace MotionPlanning.MotionPlanningComponents.Visualizations
{
    class Visualizer_Car1st : Visualizer_SimplifiedPlanarPhysicalRobots, UserInputHandler
    {
        protected Control ActiveArea_primary;
        protected bool goal_selected;
        protected bool goal_theta_selected;

        public Visualizer_Car1st(Graphics canvas, Rectangle rect)
            :base(canvas, rect)
        {
            goal_selected = false;
            goal_theta_selected = false;
        }

        override public void PlotTrajectory(Trajectory trajectory)
        {
            System_SimplifiedPlanarPhysicalRobot system = (System_SimplifiedPlanarPhysicalRobot)planner_subject.GetSystem();
            this.DrawPhysicalEnvironment(system);
            if (trajectory == null)
                return;
            double[] q_new = new double[system.Get_DOF_state()];
            List<Edge> Edges = trajectory.GetEdges();
            double time_accu = 0;
            double time_delta = 0.15;
            double time_total = 0;

            int index_duration = planner_subject.GetSystem().Get_DOF_control();
            for (int i = 0; i < Edges.Count - 1; ++i)
            {
                time_total += Edges[i].get_Value().get_ctrl_inc()[index_duration];
            }
                
            double time_beginning_of_current_edge = 0;
            double time_end_of_current_edge = 0;
            int index_of_current_edge = 0;
            while (time_accu < time_total)
            {
                time_end_of_current_edge = time_beginning_of_current_edge + Edges[index_of_current_edge].get_Value().get_ctrl_inc()[index_duration];

                if (time_accu <= time_end_of_current_edge)
                {
                    double ratio = (time_accu - time_beginning_of_current_edge) / (time_end_of_current_edge - time_beginning_of_current_edge);

                    double[] q_start = Edges[index_of_current_edge].get_entry().get_Value().get_q();
                    double[] u_inc = Edges[index_of_current_edge].get_Value().get_ctrl_inc();
                    double t_inc = u_inc[index_duration] * ratio;
                    double cost_inc = 0;
                    planner_subject.GetSystem().Propagate(q_start, u_inc, t_inc, ref q_new, ref cost_inc);
                    this.DrawPhysicalRobot(system, q_new); // this should be q_new

                    time_accu += time_delta;
                }
                else
                {
                    time_beginning_of_current_edge = time_end_of_current_edge;
                    index_of_current_edge++;
                }
            }
        }

        virtual public void Set_ActiveArea_Primary(Control control)
        {
            ActiveArea_primary = control;
            ActiveArea_primary.MouseDown += ActiveArea_primary_MouseDown;
            ActiveArea_primary.MouseUp += ActiveArea_primary_MouseUp;
            ActiveArea_primary.MouseMove += ActiveArea_primary_MouseMove;

        }

        virtual protected void ActiveArea_primary_MouseMove(object sender, MouseEventArgs e)
        {
            if (goal_selected == true || goal_theta_selected == true)
            {
                double ex = (e.X - center_x) / calibrater_x;
                double ey = (e.Y - center_y) / calibrater_y;
                double[] q_goal = planner_subject.Get_q_goal();
                double gx = q_goal[0];
                double gy = q_goal[1];
                double gt = q_goal[2];

                if (goal_selected == true)
                {
                    q_goal[0] = ex;
                    q_goal[1] = ey;
                    planner_subject.SetGoal(q_goal);
                }
                else if (goal_theta_selected == true)
                {
                    double dx = ex - gx;
                    double dy = ey - gy;
                    double length = Math.Sqrt(dx * dx + dy * dy);
                    if (length != 0)
                    {
                        double orientation = Math.Atan2(dy, dx);
                        double PI_double = Math.PI + Math.PI;
                        while (orientation > Math.PI)
                            orientation -= PI_double;
                        while (orientation < -Math.PI)
                            orientation += PI_double;
                        q_goal[2] = orientation;
                        planner_subject.SetGoal(q_goal);
                    }
                }

            }
        }

        virtual protected void ActiveArea_primary_MouseUp(object sender, MouseEventArgs e)
        {
            goal_selected = false;
            goal_theta_selected = false;
        }

        virtual protected void ActiveArea_primary_MouseDown(object sender, MouseEventArgs e)
        {
            double ex = (e.X - center_x) / calibrater_x;
            double ey = (e.Y - center_y) / calibrater_y;

            // check goal
            double[] q_goal = planner_subject.Get_q_goal();

            double dx = q_goal[0] - ex;
            double dy = q_goal[1] - ey;
            double dist2goal = Math.Sqrt(dx * dx + dy * dy);

            if (dist2goal < 0.1)
                goal_selected = true;

            // check goal orientation
            double theta = q_goal[2];
            double wheel_base = ((System_Car1st)planner_subject.GetSystem()).GetWheelbase();
            double h_x = wheel_base * Math.Cos(theta) + q_goal[0];
            double h_y = wheel_base * Math.Sin(theta) + q_goal[1];
            dx = h_x - ex;
            dy = h_y - ey;
            double dist2goalHead = Math.Sqrt(dx * dx + dy * dy);
            if (dist2goalHead < 0.1)
                goal_theta_selected = true;
        }

        public void Get_ActiveArea_Primary()
        {
            throw new NotImplementedException();
        }
    }
}
